#include <sdkeli_ls_tcp/sdkeli_ls_common_tcp.h>
#include <sdkeli_ls_tcp/sdkeli_ls1207de_parser.h>

#include <ros/ros.h>
#include <nodelet/nodelet.h>

#include <boost/thread.hpp>
#include <boost/smart_ptr.hpp>

namespace sdkeli_ls {
    class SDKeliLs1207DETcpNodelet: public nodelet::Nodelet
    {
    private:
        boost::shared_ptr<boost::thread> pPublishScanThread_;// publish scan thread
        bool enabled_;// is the nodelet running
    public:
        virtual void onInit()
        {
            ROS_INFO("Bringup SDKeliLs1207DETcp nodelet");
            enabled_ = true;
            pPublishScanThread_ = boost::make_shared<boost::thread>(boost::bind(&SDKeliLs1207DETcpNodelet::publishScan, this));
        }

        void publishScan()
        {
            /*Get node handle and prive node handle*/
            ros::NodeHandle nh = getNodeHandle();
            ros::NodeHandle nhPrivHome = getPrivateNodeHandle();

            /*Check whether hostname is provided*/
            bool isTcpConnection = false;
            std::string strHostName;
            std::string strPort;

            if(nhPrivHome.getParam("hostname", strHostName))
            {
                isTcpConnection = true;
                nhPrivHome.param<std::string>("port", strPort, "2112");
            }

            /*Get configured time limit*/
            int iTimeLimit = 5;
            nhPrivHome.param("timelimit", iTimeLimit, 5);

            bool isDataSubscribed = false;
            nhPrivHome.param("subscribe_datagram", isDataSubscribed, false);

            int iDeviceNumber = 0;
            nhPrivHome.param("device_number", iDeviceNumber, 0);

            /*Create and initialize parser*/
            sdkeli_ls::CSDKeliLs1207DEParser *pParser = new sdkeli_ls::CSDKeliLs1207DEParser();
            double param;

            if(nhPrivHome.getParam("range_min", param))
            {
                pParser->SetRangeMin(param);
            }
            if(nhPrivHome.getParam("range_max", param))
            {
                pParser->SetRangeMax(param);
            }
            if(nhPrivHome.getParam("time_increment", param))
            {
                pParser->SetTimeIncrement(param);
            }

            /*Setup TCP connection and attempt to connect/reconnect*/
            sdkeli_ls::CSDKeliLsCommon *pSDKeliLs = NULL;
            int result = sdkeli_ls::ExitError;
            while(ros::ok() && enabled_ == true)
            {
                if(pSDKeliLs != NULL)
                {
                    delete pSDKeliLs;
                }

                pSDKeliLs = new sdkeli_ls::CSDKeliLsCommonTcp(strHostName, strPort, iTimeLimit, pParser, nh, nhPrivHome);
                result = pSDKeliLs->Init();

                /*Device has been initliazed successfully*/
                while(ros::ok() && (result == sdkeli_ls::ExitSuccess) && enabled_ == true)
                {
                    ros::spinOnce();
                    result = pSDKeliLs->LoopOnce();
                }

                if(result == sdkeli_ls::ExitFatal)
                {
                    ROS_ERROR("Grab laser data error:ExitFatal");
                    if(pSDKeliLs != NULL)
                    {
                        delete pSDKeliLs;
                    }
                    return ;
                }
            }

            if(pSDKeliLs != NULL)
            {
                delete pSDKeliLs;
            }

            if(pParser != NULL)
            {
                delete pParser;
            }
        }

        ~SDKeliLs1207DETcpNodelet()
        {
            enabled_ = false;
            pPublishScanThread_->join();
        }
    };
}/*sdkeli_ls*/

// watch the capitalization carefully
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(sdkeli_ls::SDKeliLs1207DETcpNodelet, nodelet::Nodelet)
